#!/usr/bin/env python3

import rospy
from DrEmpower_can.msg import motion_follow_aid_multi
# import DrEmpower as dr

def callback(data):
    import DrEmpower as dr
    dr.motion_aid_multi(id_list=data.id_list, angle_list=data.angle_list, speed_list=data.speed_list, 
                        angle_err_list=data.angle_err_list, speed_err_list=data.speed_err_list, torque_list=data.torque_list)
    rospy.loginfo("motion_follow_aid_multi_node")


def listener():
    rospy.init_node("motion_follow_aid_multi", anonymous=True)
    rospy.Subscriber("motion_follow_aid_multi", motion_follow_aid_multi, callback)
    rospy.spin()


if __name__ == '__main__':
    listener()

